#include <bitset>
#include "scenes_judge.hpp"

using namespace std;
using namespace tinyxml2;

#define BIT_(x) (((uint64_t)1) << (x))
#define TestBit_(x, i) ((x & BIT_(i)) != 0)
#define MANUL_BIT 4
#define HALF_BIT 22
#define AUTO_ONLINE_BIT 23

namespace AgvObstaAvoid {

ScenesJudge::ScenesJudge(): 
    workstate(0)
{}

// void ScenesJudge::init()
// {
//      // Create subscribers
//     odom_sub_ = this->create_subscription<agv_msgs::msg::OdometryLite>(
//         "/odom", 10, std::bind(&ScenesJudge::OdomCallback, this, std::placeholders::_1));
    
//     load_sub_ = this->create_subscription<agv_msgs::msg::PeriphStatus>(
//         "/periphStatus", 10, std::bind(&ScenesJudge::LoadCallback, this, std::placeholders::_1));
    
//     path_sub_ = this->create_subscription<agv_msgs::msg::AgvAutoStatus>(
//         "/auto_run_status", 10, std::bind(&ScenesJudge::PathCallback, this, std::placeholders::_1));
// }

bool ScenesJudge::ScenesReadXml(XMLElement* element) {
    
    RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), "read agv avoidance obstacle xml scenes plan");
    try
    {
        plan_list.clear();
        XMLElement* plan_node = element->FirstChildElement();
        
        while (plan_node)
        {
            cPlan plan_item;
            plan_item.name = plan_node->Attribute("name") ? plan_node->Attribute("name") : EVENT_STRING;
            plan_item.default_state = plan_node->Attribute("defaultstate") ? stoi(plan_node->Attribute("defaultstate")) : EVENT_INT;
            plan_item.velocity_state = plan_node->Attribute("velocity") ? stoi(plan_node->Attribute("velocity")) : EVENT_INT;
            plan_item.load_state = plan_node->Attribute("loadstate") ? stoi(plan_node->Attribute("loadstate")) : EVENT_INT;
            plan_item.traj_state = plan_node->Attribute("trajtype") ? stoi(plan_node->Attribute("trajtype")) : EVENT_INT;
            plan_item.bit_state = plan_node->Attribute("custom") ? stoi(plan_node->Attribute("custom")) : EVENT_INT;

            XMLElement* equip_node = plan_node->FirstChildElement();
            std::vector<cEquip> equip_list;
            while (equip_node)
            {
                cEquip equip_item;
                equip_item.name = equip_node->Attribute("name") ? stoi(equip_node->Attribute("name")) : EVENT_INT;
                equip_item.type = equip_node->Attribute("type") ? equip_node->Attribute("type") : EVENT_STRING;
                equip_item.area = equip_node->Attribute("area") ? stoi(equip_node->Attribute("area")) : EVENT_INT;
                // level 3 obstacle avoidance area is on by default
                equip_item.warnenable = equip_node->Attribute("warnenable") ? stoi(equip_node->Attribute("warnenable")) : 1;
                equip_item.slowenable = equip_node->Attribute("slowenable") ? stoi(equip_node->Attribute("slowenable")) : 1;
                equip_item.stopenable = equip_node->Attribute("stopenable") ? stoi(equip_node->Attribute("stopenable")) : 1;

                equip_list.push_back(equip_item);
                equip_node = equip_node->NextSiblingElement();
            }

            plan_item.equiplist = equip_list;
            
            // Check for duplicates of added programme names
            if (plan_list.find(plan_item.name) != plan_list.end())
            {
                RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), 
                    "The same scheme name is already available in the avoidance scheme, please check");
                return false;
            }

            plan_list[plan_item.name] = plan_item;
            plan_node = plan_node->NextSiblingElement();
        }
    }
    catch(const std::exception& e)
    {
        std::cerr << e.what() << '\n';
        RCLCPP_ERROR(rclcpp::get_logger("ScenesJudge"), "xml scenes read is error");
        return false;
    }

    RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), 
        "scenes init xml param successed, show scenes plan item");
    for (auto item = plan_list.begin(); item != plan_list.end(); item++)
    {
        bool ret = ShowScenesPlan(&(item->second));
        if (!ret)
        {
            RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), "scenes read xml param error");
            return false;
        }
        
        // The obstacle avoidance scheme in manual control mode uses the default
        if (item->second.default_state == 1)
        {
            RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), 
                "agv manaul control, use default scenes: %s", item->first.c_str());
            scenes_name = item->first;
            
            vector<cEquip> scan_equip;
            vector<cEquip> dido_equip;
            vector<cEquip> came_equip;
            for (size_t i = 0; i < item->second.equiplist.size(); i++)
            {
                if (item->second.equiplist[i].type == "graphic")
                {
                    scan_equip.push_back(item->second.equiplist[i]);
                } else if (item->second.equiplist[i].type == "dido")
                {
                    dido_equip.push_back(item->second.equiplist[i]);
                }  else if (item->second.equiplist[i].type == "graphic_3d")
                {
                    came_equip.push_back(item->second.equiplist[i]);
                }
            }
            scan_equip_manual = scan_equip;
            dido_equip_manual = dido_equip;
            came_equip_manual = came_equip;
        }
    }
    return true;
}

bool ScenesJudge::ShowScenesPlan(cPlan* plan_item) {
    
    RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), 
        "plan name: %s velocity: %d load: %d defaultstate: %d traj_state: %d bits: %d", 
                plan_item->name.c_str(), 
                plan_item->velocity_state, 
                plan_item->load_state, 
                plan_item->default_state,
                plan_item->traj_state, 
                plan_item->bit_state);
    
    if (plan_item->name == EVENT_STRING || 
        plan_item->velocity_state == EVENT_INT || 
        plan_item->load_state == EVENT_INT || 
        plan_item->default_state == EVENT_INT || 
        plan_item->traj_state == EVENT_INT || 
        plan_item->bit_state == EVENT_INT)
    {
        return false;
    }
    
    for (size_t n = 0; n < plan_item->equiplist.size(); n++)
    {
        RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), 
            "equip name: %d type:%s area:%d warnenable:%d slowenable:%d stopenable:%d", 
                    plan_item->equiplist[n].name,
                    plan_item->equiplist[n].type.c_str(),
                    plan_item->equiplist[n].area, 
                    plan_item->equiplist[n].warnenable,
                    plan_item->equiplist[n].slowenable, 
                    plan_item->equiplist[n].stopenable);
        
        if (plan_item->equiplist[n].name == EVENT_INT || 
            plan_item->equiplist[n].type == EVENT_STRING || 
            plan_item->equiplist[n].area == EVENT_INT)
        {
            return false;
        }
    }
    return true;
}

ScenesJudge::AgvState ScenesJudge::GetAgvStateFromPlan(const ScenesJudge::cPlan& plan) {
    AgvState agv_state_item;
    agv_state_item.load = plan.load_state;
    agv_state_item.velocity = plan.velocity_state;
    agv_state_item.traj_type = plan.traj_state;
    agv_state_item.bits = plan.bit_state;
    return agv_state_item;
}

void ScenesJudge::ShowAgvState(const ScenesJudge::AgvState& state) {
    RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), 
        "current agv state velocity: %d load: %d traj_type: %d bits: %d", 
                state.velocity, 
                state.load, 
                state.traj_type,
                state.bits);
    return;
}

// Manual switching of the obstacle avoidance scheme call function
bool ScenesJudge::ScenesSwitch(const string planname) {
    
    vector<cEquip> scan_equip;
    vector<cEquip> dido_equip;
    vector<cEquip> came_equip;

    if (plan_list.find(planname) != plan_list.end()) {
        auto item = plan_list.find(planname);
        RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"),
         "agv manual state use avoid scenes is: %s", planname.c_str());
        for (size_t i = 0; i < item->second.equiplist.size(); i++)
        {
            if (item->second.equiplist[i].type == "graphic")
            {
                scan_equip.push_back(item->second.equiplist[i]);
            } else if (item->second.equiplist[i].type == "dido")
            {
                dido_equip.push_back(item->second.equiplist[i]);
            } else if (item->second.equiplist[i].type == "graphic_3d")
            {
                came_equip.push_back(item->second.equiplist[i]);
            }
        }
        scenes_name = item->first;
        scan_equip_manual = scan_equip;
        dido_equip_manual = dido_equip;
        came_equip_manual = came_equip;

        return true;
    } 

    return false;
}

bool ScenesJudge::AgvStatePolling() 
{
    if (agv_state_main != agv_state)
    {
        RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), "agv state has been changed");
        RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), 
            "agv last state is velocity: %d load: %d traj_type: %d bits: %d", 
                    agv_state_main.velocity, 
                    agv_state_main.load, 
                    agv_state_main.traj_type, 
                    agv_state_main.bits);
        return true;
    }
    return false;
}

bool ScenesJudge::AgvBitJudge(AgvState agv_state, AgvState state_plan)
{
    if ((agv_state.bits != 0) && TestBit_(agv_state.bits, state_plan.bits - 1))
    {
        RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), 
            "TestBit_: %d", TestBit_(agv_state.bits, state_plan.bits - 1));
        return true;
    }

    return false;
}

bool ScenesJudge::AgvStateJudge(ScenesJudge::AgvState agv_state, ScenesJudge::AgvState state_plan)
{
    if ( (state_plan.load != 0) || (state_plan.velocity != 0) || (state_plan.traj_type != 0) ) {
        bool load = false;
        bool velocity = false; 
        bool type = false;

        if ((state_plan.velocity == 0) || (agv_state.velocity == state_plan.velocity))
            velocity = true;

        if ((state_plan.load == 0) || (agv_state.load == state_plan.load))
            load = true;

        if ((state_plan.traj_type == 0) || (agv_state.traj_type == state_plan.traj_type))
            type = true;
        
        if (load && velocity && type)
            return true;
    }

    return false;
}

ScenesJudge::EquipDispatch ScenesJudge::PlanEquipDistribute() 
{
    EquipDispatch equip_disp;

    ShowAgvState(agv_state);
    RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), "distribution of equipment by the programme");
    
    // Use of the Map Properties Programme
    if (scenes_from_map.empty()) {
        RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), "The map attribute programme is empty");
    } else {
        RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), "use map attributes: %s", scenes_from_map.c_str());
        if (plan_list.find(scenes_from_map) != plan_list.end()) {
            auto item = plan_list.find(scenes_from_map);
            for (size_t i = 0; i < item->second.equiplist.size() ; i++)
            {
                if (item->second.equiplist[i].type == "graphic") {
                    equip_disp.scan_equip.push_back(item->second.equiplist[i]);
                } else if (item->second.equiplist[i].type == "dido") {
                    equip_disp.dido_equip.push_back(item->second.equiplist[i]);
                } else if (item->second.equiplist[i].type == "graphic_3d") {
                    equip_disp.came_equip.push_back(item->second.equiplist[i]);
                }
            }
            scenes_name = item->first;
            agv_state_main = agv_state;
            return equip_disp;
        } else {
            RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), 
                "this map attribute scenes is not available in the programme");
        }
    }
    
    // 从避障文件中查询方案
    for (auto item = plan_list.begin(); item != plan_list.end(); item++)
    {
        AgvState from_plan = GetAgvStateFromPlan(item->second);
        // 优先进行bits的判断   0-0-0-5
        if (from_plan.bits != 0) {
            RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), 
                "programme of the current query: %s velocity:%d load:%d traj_type:%d bits:%d", 
                        item->first.c_str(),
                        from_plan.velocity,
                        from_plan.load,
                        from_plan.traj_type,
                        from_plan.bits);

            if (AgvBitJudge(agv_state, from_plan)) {
                RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), "find suit plan, use %s", item->first.c_str());
                for (size_t i = 0; i < item->second.equiplist.size() ; i++)
                {
                    if (item->second.equiplist[i].type == "graphic") {
                        equip_disp.scan_equip.push_back(item->second.equiplist[i]);
                    } else if (item->second.equiplist[i].type == "dido") {
                        equip_disp.dido_equip.push_back(item->second.equiplist[i]);
                    } else if (item->second.equiplist[i].type == "graphic_3d") {
                        equip_disp.came_equip.push_back(item->second.equiplist[i]);
                    }
                }
                scenes_name = item->first;
                agv_state_main = agv_state;
                return equip_disp;
            }
        }
    }

    for (auto item = plan_list.begin(); item != plan_list.end(); item++)
    {
        AgvState from_plan = GetAgvStateFromPlan(item->second);
        RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), 
            "programme of the current query: %s velocity:%d load:%d traj_type:%d bits:%d", 
                    item->first.c_str(),
                    from_plan.velocity,
                    from_plan.load,
                    from_plan.traj_type,
                    from_plan.bits);

        if (AgvStateJudge(agv_state, from_plan))
        {
            RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), "find suit plan, use %s", item->first.c_str());
            for (size_t i = 0; i < item->second.equiplist.size() ; i++)
            {
                if (item->second.equiplist[i].type == "graphic")
                {
                    equip_disp.scan_equip.push_back(item->second.equiplist[i]);
                } else if (item->second.equiplist[i].type == "dido")
                {
                    equip_disp.dido_equip.push_back(item->second.equiplist[i]);
                } else if (item->second.equiplist[i].type == "graphic_3d") 
                {
                    equip_disp.came_equip.push_back(item->second.equiplist[i]);
                }
            }
            scenes_name = item->first;
            agv_state_main = agv_state;
            return equip_disp;
        }
    }
    
    // Use of default programmes
    for (auto item = plan_list.begin(); item != plan_list.end(); item++)
    {
        if (item->second.default_state == 1)
        {
            RCLCPP_INFO(rclcpp::get_logger("ScenesJudge"), "Use of default programmes: %s",
                 item->first.c_str());
            for (size_t i = 0; i < item->second.equiplist.size() ; i++)
            {
                if (item->second.equiplist[i].type == "graphic")
                {
                    equip_disp.scan_equip.push_back(item->second.equiplist[i]);
                } else if (item->second.equiplist[i].type == "dido")
                {
                    equip_disp.dido_equip.push_back(item->second.equiplist[i]);
                } else if (item->second.equiplist[i].type == "graphic_3d") 
                {
                    equip_disp.came_equip.push_back(item->second.equiplist[i]);
                }
            }
            scenes_name = item->first;
            agv_state_main = agv_state;
            return equip_disp;
        }
    }
    
    return equip_disp;
}

void ScenesJudge::OdomCallback(const agv_msgs::msg::OdometryLite::SharedPtr odom_msg) 
{
    double velocity_level = abs(odom_msg->twist.linear.x);
    double angular_level = odom_msg->twist.angular.z;
    if (velocity_level < 0.5) {
        agv_state.velocity = 1;
    } else if (velocity_level >= 0.5 && velocity_level <= 1.0 ) {
        agv_state.velocity = 2;
    } else if (velocity_level > 1.0) {
        agv_state.velocity = 3;
    }
    
    return;
}

void ScenesJudge::LoadCallback(const agv_msgs::msg::PeriphStatus::SharedPtr load_msg) 
{
    bool load = int(load_msg->loaded);
    if (load) {
        agv_state.load = 2; //负载
    } else {
        agv_state.load = 1; //空载
    }
    
    return;
}

void ScenesJudge::PathCallback(const agv_msgs::msg::AgvAutoStatus::SharedPtr path_msg) 
{
    int trajtype = path_msg->trajtype;
    agv_state.bits = path_msg->bits;

    workstate = path_msg->workstate;
    /* 地图属性调用场景 */
    if (stoi(path_msg->scenes) > 0) {
        string plan_name = "plan";
        scenes_from_map = plan_name + path_msg->scenes;
    } else {
        scenes_from_map.clear();
    }

    switch (trajtype)
    {
        case TRAJ_LINE:
            agv_state.traj_type = 1;
            break;
        case TRAJ_BEZIER:
            agv_state.traj_type = 2;
            break;
        case TRAJ_SIDE:
            agv_state.traj_type = 3;
            break;
        case TRAJ_SPIN:
            agv_state.traj_type = 4;
            break;
        case TRAJ_BSPLINE:
            agv_state.traj_type = 5;
            break;
    }
    
    return;
}

} // namespace AgvObstaAvoid end